import numpy as np


def build_local_lidar_map(lidar_points, robot_size, map_size = (100, 100)):
    grid = np.full(map_size, 0.5)  # 初始占用概率为 0.5
    map_x, map_y = map_size
    for point in lidar_points:
        px, py, pz = point
        if not (-1.5 <= px <= 1.5 and -1.5 <= py <= 1.5):
            continue
        if pz < -0.05:
            continue
        x = int(map_x // 2 - px * 30)
        y = int(map_y // 2 - py * 30)
        obts_center = (x,y)
        grid[obts_center] = 0.6
        

    return grid


def build_local_lidar_update_map(local_map, lidar_points, fusepose_xyz, fuse_pose_rpy, map_size = (100, 100)):
    grid = local_map  # 初始占用概率为 0.5
    map_x, map_y = map_size
    for point in lidar_points:
        px, py, pz = point
        if not (-1.5 <= px <= 1.5 and -1.5 <= py <= 1.5):
            continue
        if pz < -0.05:
            continue
        x = int(map_x // 2 - px * 30)
        y = int(map_y // 2 - py * 30)
        obts_center = (x,y)
        grid[obts_center] = 0.9

    return grid



def update_local_lidar_map():
    pass